//Laser_odometry
//该线程作用:通过帧间匹配实现lidar里程计的功能;

#include <cmath>
#include </home/sishubin/SLAMcodes/My_Local_Repo/Laser_fusion/src/laser_fusion/include/laser_fusion/common.h>
#include <nav_msgs/Odometry.h>
#include <opencv/cv.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl_conversions/pcl_conversions.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>


//一个点云周期
const float scanPeriod = 0.1;

//跳帧数，控制发给laserMapping的频率
const int skipFrameNum = 1;
bool systemInited = false;

//时间戳信息
double timeCornerPointsSharp = 0;
double timeCornerPointsLessSharp = 0;
double timeSurfPointsFlat = 0;
double timeSurfPointsLessFlat = 0;
double timeLaserCloudFullRes = 0;
double timeImuTrans = 0;

//消息接收标志
bool newCornerPointsSharp = false;
bool newCornerPointsLessSharp = false;
bool newSurfPointsFlat = false;
bool newSurfPointsLessFlat = false;
bool newLaserCloudFullRes = false;
bool newImuTrans = false;

//receive sharp points
pcl::PointCloud<PointType>::Ptr cornerPointsSharp(new pcl::PointCloud<PointType>());
//receive less sharp points
pcl::PointCloud<PointType>::Ptr cornerPointsLessSharp(new pcl::PointCloud<PointType>());
//receive flat points
pcl::PointCloud<PointType>::Ptr surfPointsFlat(new pcl::PointCloud<PointType>());
//receive less flat points
pcl::PointCloud<PointType>::Ptr surfPointsLessFlat(new pcl::PointCloud<PointType>());
//receive all points
pcl::PointCloud<PointType>::Ptr laserCloudFullRes(new pcl::PointCloud<PointType>());
//receive imu info
pcl::PointCloud<pcl::PointXYZ>::Ptr imuTrans(new pcl::PointCloud<pcl::PointXYZ>());
//less sharp points of last frame
pcl::PointCloud<PointType>::Ptr laserCloudCornerLast(new pcl::PointCloud<PointType>());
//less flat points of last frame
pcl::PointCloud<PointType>::Ptr laserCloudSurfLast(new pcl::PointCloud<PointType>());
//kd-tree built by less sharp points of last frame
pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN<PointType>());
//kd-tree built by less flat points of last frame
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfLast(new pcl::KdTreeFLANN<PointType>());
//保存前一个节点发过来的未经处理过的特征点
pcl::PointCloud<PointType>::Ptr laserCloudOri(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr coeffSel(new pcl::PointCloud<PointType>());


int laserCloudCornerLastNum;//默认初始化为0
int laserCloudSurfLastNum;

//save 2 corner points index searched
float pointSearchCornerInd1[40000];
float pointSearchCornerInd2[40000];

//save 3 surf points index searched
float pointSearchSurfInd1[40000];
float pointSearchSurfInd2[40000];
float pointSearchSurfInd3[40000];

//点云第一个点对应的RPY
float imuRollStart = 0, imuPitchStart = 0, imuYawStart = 0;
//点云最后一个点对应的RPY
float imuRollLast = 0, imuPitchLast = 0, imuYawLast = 0;
//点云最后一个点相对于第一个点的畸变位移
float imuShiftFromStartX = 0, imuShiftFromStartY = 0, imuShiftFromStartZ = 0;
//点云最后一个点相对于第一个点的畸变速度
float imuVeloFromStartX = 0, imuVeloFromStartY = 0, imuVeloFromStartZ = 0;

//当前帧相对上一帧的状态转移量，in the local frame
//存放的参数依次是:俯仰角, ?? , 横滚角和位移XYZ
float transform[6] = {0};
//当前帧相对于第一帧的状态转移量，in the global frame
float transformSum[6] = {0};


void laserCloudSharpHandler(const sensor_msgs::PointCloud2::ConstPtr& cornerPointsSharp2)
{
    timeCornerPointsSharp = cornerPointsSharp2->header.stamp.toSec();
    cornerPointsSharp->clear();
    //将点云的ros消息转换成pcl格式
    pcl::fromROSMsg(*cornerPointsSharp2,*cornerPointsSharp);
    std::vector<int>indices;
    //去除Nan点
    pcl::removeNaNFromPointCloud(*cornerPointsSharp,*cornerPointsSharp,indices);
    newCornerPointsSharp = true;
}

void laserCloudLessSharpHandler(const sensor_msgs::PointCloud2::ConstPtr& cornerPointsLessSharp2)
{
    timeCornerPointsLessSharp = cornerPointsLessSharp2->header.stamp.toSec();
    cornerPointsLessSharp->clear();
    pcl::fromROSMsg(*cornerPointsLessSharp2, *cornerPointsLessSharp);
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(*cornerPointsLessSharp,*cornerPointsLessSharp, indices);
    newCornerPointsLessSharp = true;
}

void laserCloudFlatHandler(const sensor_msgs::PointCloud2::ConstPtr& surfPointsFlat2)
{
    timeSurfPointsFlat = surfPointsFlat2->header.stamp.toSec();
    surfPointsFlat->clear();
    pcl::fromROSMsg(*surfPointsFlat2, *surfPointsFlat);
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(*surfPointsFlat,*surfPointsFlat, indices);
    newSurfPointsFlat = true;
}

void laserCloudLessFlatHandler(const sensor_msgs::PointCloud2::ConstPtr& surfPointsLessFlat2)
{
    timeSurfPointsLessFlat = surfPointsLessFlat2->header.stamp.toSec();
    surfPointsLessFlat->clear();
    pcl::fromROSMsg(*surfPointsLessFlat2, *surfPointsLessFlat);
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(*surfPointsLessFlat,*surfPointsLessFlat, indices);
    newSurfPointsLessFlat = true;
}

void laserCloudFullResHandler(const sensor_msgs::PointCloud2::ConstPtr& laserCloudFullRes2)
{
    timeLaserCloudFullRes = laserCloudFullRes2->header.stamp.toSec();
    laserCloudFullRes->clear();
    pcl::fromROSMsg(*laserCloudFullRes2, *laserCloudFullRes);
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(*laserCloudFullRes,*laserCloudFullRes, indices);
    newLaserCloudFullRes = true;
}

void imuTransHandler(const sensor_msgs::PointCloud2::ConstPtr& imuTrans2)
{
    timeImuTrans = imuTrans2->header.stamp.toSec();

    imuTrans->clear();
    pcl::fromROSMsg(*imuTrans2, *imuTrans);

    //根据接收到的消息提取imu信息
    imuPitchStart = imuTrans->points[0].x;
    imuYawStart = imuTrans->points[0].y;
    imuRollStart = imuTrans->points[0].z;

    imuPitchLast = imuTrans->points[1].x;
    imuYawLast = imuTrans->points[1].y;
    imuRollLast = imuTrans->points[1].z;

    imuShiftFromStartX = imuTrans->points[2].x;
    imuShiftFromStartY = imuTrans->points[2].y;
    imuShiftFromStartZ = imuTrans->points[2].z;

    imuVeloFromStartX = imuTrans->points[3].x;
    imuVeloFromStartY = imuTrans->points[3].y;
    imuVeloFromStartZ = imuTrans->points[3].z;

    newImuTrans = true;
}

//当前点云中的点相对第一个点去除因 匀速运动 产生的畸变，效果相当于得到在点云扫描开始位置静止扫描得到的点云
void TransformToStart(PointType const * const pi, PointType * const po)
{
    //插值系数计算，云中每个点的相对时间/点云周期(0.1)
    float s = 10 * (pi->intensity - int(pi->intensity));

    //线性插值：根据每个点在点云中的相对位置关系，乘以相应的旋转平移系数
    float rx = s * transform[0];
    float ry = s * transform[1];
    float rz = s * transform[2];
    float tx = s * transform[3];
    float ty = s * transform[4];
    float tz = s * transform[5];

    //平移后绕z轴旋转（-rz）
    float x1 = cos(rz) * (pi->x - tx) + sin(rz) * (pi->y - ty);
    float y1 = -sin(rz) * (pi->x - tx) + cos(rz) * (pi->y - ty);
    float z1 = (pi->z - tz);

    //绕x轴旋转（-rx）
    float x2 = x1;
    float y2 = cos(rx) * y1 + sin(rx) * z1;
    float z2 = -sin(rx) * y1 + cos(rx) * z1;

    //绕y轴旋转（-ry）
    po->x = cos(ry) * x2 - sin(ry) * z2;
    po->y = y2;
    po->z = sin(ry) * x2 + cos(ry) * z2;
    po->intensity = pi->intensity;

}

//将上一帧点云中的点相对结束位置去除因匀速运动产生的畸变，效果相当于得到在点云扫描结束位置静止扫描得到的点云
void TransformToEnd(PointType const * const pi, PointType * const po)
{
    //插值系数计算
    float s = 10 * (pi->intensity - int(pi->intensity));

    float rx = s * transform[0];
    float ry = s * transform[1];
    float rz = s * transform[2];
    float tx = s * transform[3];
    float ty = s * transform[4];
    float tz = s * transform[5];

    //平移后绕z轴旋转（-rz）
    float x1 = cos(rz) * (pi->x - tx) + sin(rz) * (pi->y - ty);
    float y1 = -sin(rz) * (pi->x - tx) + cos(rz) * (pi->y - ty);
    float z1 = (pi->z - tz);

    //绕x轴旋转（-rx）
    float x2 = x1;
    float y2 = cos(rx) * y1 + sin(rx) * z1;
    float z2 = -sin(rx) * y1 + cos(rx) * z1;

    //绕y轴旋转（-ry）
    float x3 = cos(ry) * x2 - sin(ry) * z2;
    float y3 = y2;
    float z3 = sin(ry) * x2 + cos(ry) * z2;//求出了相对于起始点校正的坐标

    rx = transform[0];
    ry = transform[1];
    rz = transform[2];
    tx = transform[3];
    ty = transform[4];
    tz = transform[5];

    //绕y轴旋转（ry）
    float x4 = cos(ry) * x3 + sin(ry) * z3;
    float y4 = y3;
    float z4 = -sin(ry) * x3 + cos(ry) * z3;

    //绕x轴旋转（rx）
    float x5 = x4;
    float y5 = cos(rx) * y4 - sin(rx) * z4;
    float z5 = sin(rx) * y4 + cos(rx) * z4;

    //绕z轴旋转（rz），再平移
    float x6 = cos(rz) * x5 - sin(rz) * y5 + tx;
    float y6 = sin(rz) * x5 + cos(rz) * y5 + ty;
    float z6 = z5 + tz;

    //平移后绕z轴旋转（imuRollStart）
    float x7 = cos(imuRollStart) * (x6 - imuShiftFromStartX) 
            - sin(imuRollStart) * (y6 - imuShiftFromStartY);
    float y7 = sin(imuRollStart) * (x6 - imuShiftFromStartX) 
            + cos(imuRollStart) * (y6 - imuShiftFromStartY);
    float z7 = z6 - imuShiftFromStartZ;

    //绕x轴旋转（imuPitchStart）
    float x8 = x7;
    float y8 = cos(imuPitchStart) * y7 - sin(imuPitchStart) * z7;
    float z8 = sin(imuPitchStart) * y7 + cos(imuPitchStart) * z7;

    //绕y轴旋转（imuYawStart）
    float x9 = cos(imuYawStart) * x8 + sin(imuYawStart) * z8;
    float y9 = y8;
    float z9 = -sin(imuYawStart) * x8 + cos(imuYawStart) * z8;

    //绕y轴旋转（-imuYawLast）
    float x10 = cos(imuYawLast) * x9 - sin(imuYawLast) * z9;
    float y10 = y9;
    float z10 = sin(imuYawLast) * x9 + cos(imuYawLast) * z9;

    //绕x轴旋转（-imuPitchLast）
    float x11 = x10;
    float y11 = cos(imuPitchLast) * y10 + sin(imuPitchLast) * z10;
    float z11 = -sin(imuPitchLast) * y10 + cos(imuPitchLast) * z10;

    //绕z轴旋转（-imuRollLast）
    po->x = cos(imuRollLast) * x11 + sin(imuRollLast) * y11;
    po->y = -sin(imuRollLast) * x11 + cos(imuRollLast) * y11;
    po->z = z11;
    //只保留线号
    po->intensity = int(pi->intensity);

}

//相对于第一个点云即原点，积累旋转量
void AccumulateRotation(float cx, float cy, float cz, float lx, float ly, float lz, float &ox, float &oy, float &oz)
{
    float srx = cos(lx)*cos(cx)*sin(ly)*sin(cz) - cos(cx)*cos(cz)*sin(lx) - cos(lx)*cos(ly)*sin(cx);
    ox = -asin(srx);

    float srycrx = sin(lx)*(cos(cy)*sin(cz) - cos(cz)*sin(cx)*sin(cy)) + cos(lx)*sin(ly)*(cos(cy)*cos(cz) 
                + sin(cx)*sin(cy)*sin(cz)) + cos(lx)*cos(ly)*cos(cx)*sin(cy);
    float crycrx = cos(lx)*cos(ly)*cos(cx)*cos(cy) - cos(lx)*sin(ly)*(cos(cz)*sin(cy) 
                - cos(cy)*sin(cx)*sin(cz)) - sin(lx)*(sin(cy)*sin(cz) + cos(cy)*cos(cz)*sin(cx));
    oy = atan2(srycrx / cos(ox), crycrx / cos(ox));

    float srzcrx = sin(cx)*(cos(lz)*sin(ly) - cos(ly)*sin(lx)*sin(lz)) + cos(cx)*sin(cz)*(cos(ly)*cos(lz) 
                + sin(lx)*sin(ly)*sin(lz)) + cos(lx)*cos(cx)*cos(cz)*sin(lz);
    float crzcrx = cos(lx)*cos(lz)*cos(cx)*cos(cz) - cos(cx)*sin(cz)*(cos(ly)*sin(lz) 
                - cos(lz)*sin(lx)*sin(ly)) - sin(cx)*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx));
    oz = atan2(srzcrx / cos(ox), crzcrx / cos(ox));
}

//利用IMU修正旋转量，根据起始欧拉角，当前点云的欧拉角修正
void PluginIMURotation(float bcx, float bcy, float bcz, float blx, float bly, float blz, 
                       float alx, float aly, float alz, float &acx, float &acy, float &acz)
{
  float sbcx = sin(bcx);
  float cbcx = cos(bcx);
  float sbcy = sin(bcy);
  float cbcy = cos(bcy);
  float sbcz = sin(bcz);
  float cbcz = cos(bcz);

  float sblx = sin(blx);
  float cblx = cos(blx);
  float sbly = sin(bly);
  float cbly = cos(bly);
  float sblz = sin(blz);
  float cblz = cos(blz);

  float salx = sin(alx);
  float calx = cos(alx);
  float saly = sin(aly);
  float caly = cos(aly);
  float salz = sin(alz);
  float calz = cos(alz);

  float srx = -sbcx*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly) 
            - cbcx*cbcz*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 
            - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 
            - cbcx*sbcz*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 
            - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz);
  acx = -asin(srx);

  float srycrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 
               - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 
               - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 
               - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) 
               + cbcx*sbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);
  float crycrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 
               - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) 
               - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 
               - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 
               + cbcx*cbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);
  acy = atan2(srycrx / cos(acx), crycrx / cos(acx));
  
  float srzcrx = sbcx*(cblx*cbly*(calz*saly - caly*salx*salz) 
               - cblx*sbly*(caly*calz + salx*saly*salz) + calx*salz*sblx) 
               - cbcx*cbcz*((caly*calz + salx*saly*salz)*(cbly*sblz - cblz*sblx*sbly) 
               + (calz*saly - caly*salx*salz)*(sbly*sblz + cbly*cblz*sblx) 
               - calx*cblx*cblz*salz) + cbcx*sbcz*((caly*calz + salx*saly*salz)*(cbly*cblz 
               + sblx*sbly*sblz) + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) 
               + calx*cblx*salz*sblz);
  float crzcrx = sbcx*(cblx*sbly*(caly*salz - calz*salx*saly) 
               - cblx*cbly*(saly*salz + caly*calz*salx) + calx*calz*sblx) 
               + cbcx*cbcz*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) 
               + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) 
               + calx*calz*cblx*cblz) - cbcx*sbcz*((saly*salz + caly*calz*salx)*(cblz*sbly 
               - cbly*sblx*sblz) + (caly*salz - calz*salx*saly)*(cbly*cblz + sblx*sbly*sblz) 
               - calx*calz*cblx*sblz);
  acz = atan2(srzcrx / cos(acx), crzcrx / cos(acx));
}


int main(int argc, char** argv)
{
    ros::init(argc,argv,"laser_odometry");
    ros::NodeHandle nh;
    ROS_INFO("Laser_odometry...");

    //订阅scan_process发布的/laser_cloud_sharp话题,调用laserCloudSharpHandler函数
    ros::Subscriber subCornerPointsSharp = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_sharp",2,laserCloudSharpHandler);
    //订阅scan_process发布的/laser_cloud_less_sharp话题,调用laserCloudLessSharpHandler函数
    ros::Subscriber subCornerPointsLessSharp = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp",2,laserCloudLessSharpHandler);
    //订阅scan_process发布的/laser_cloud_flat话题,调用laserCloudFlatHandler函数
    ros::Subscriber subSurfPointsFlat = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_flat", 2, laserCloudFlatHandler);
    //订阅scan_process发布的/laser_cloud_less_flat话题,调用laserCloudLessFlatHandler函数
    ros::Subscriber subSurfPointsLessFlat = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 2, laserCloudLessFlatHandler);
    //订阅scan_process发布的所有点云信息
    ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2> ("/laser_point_cloud", 2, laserCloudFullResHandler);
    //订阅scan_process发布的/imu_trans消息
    ros::Subscriber subImuTrans = nh.subscribe<sensor_msgs::PointCloud2> ("/imu_trans", 5, imuTransHandler);

    //发布消息
    //点云信息
    ros::Publisher pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2);
    ros::Publisher pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2);
    ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2> ("/laser_point_cloud_odom", 2);
    //里程计信息
    ros::Publisher pubLaserOdometry = nh.advertise<nav_msgs::Odometry> ("/laser_odom_to_init", 5);

    nav_msgs::Odometry laserOdometry;
    laserOdometry.header.frame_id = "rslidar_init";
    laserOdometry.child_frame_id = "laser_odom";

    tf::TransformBroadcaster tfBroadcaster;//坐标变换的发布器
    tf::StampedTransform laserOdometryTrans;//记录里程计的姿态变换
    laserOdometryTrans.frame_id_ = "rslidar_init";
    laserOdometryTrans.child_frame_id_ = "laser_odom";

    std::vector<int> pointSearchInd;//搜索到的点序
    std::vector<float> pointSearchSqDis;//搜索到的点平方距离

    //pointOri:原始点; pointSel:选中的特征点;
    // tripod1, tripod2, tripod3:特征点的对应点
    PointType pointOri, pointSel, tripod1, tripod2, tripod3, pointProj, coeff;

    //退化标志
    bool isDegenerate = false;

    //P矩阵，预测矩阵
    cv::Mat matP(6, 6, CV_32F, cv::Scalar::all(0));

    int frameCount = skipFrameNum;

    ros::Rate rate(100);

    bool status = ros::ok();

    while(status)
    {
        //查看topic缓存区是否存在消息,有消息就传入回调函数,没有的话则继续向后执行
        // 进行一次回调函数的调用（完成当前时刻的msg的订阅和上一时刻的处理后的msg的发布)
        ros::spinOnce();
        //fabs()函数:求绝对值
        if(newCornerPointsSharp && newCornerPointsLessSharp && newSurfPointsFlat && 
            newSurfPointsLessFlat && newLaserCloudFullRes && newImuTrans &&
            fabs(timeCornerPointsSharp - timeSurfPointsLessFlat) < 0.005 &&
            fabs(timeCornerPointsLessSharp - timeSurfPointsLessFlat) < 0.005 &&
            fabs(timeSurfPointsFlat - timeSurfPointsLessFlat) < 0.005 &&
            fabs(timeLaserCloudFullRes - timeSurfPointsLessFlat) < 0.005 &&
            fabs(timeImuTrans - timeSurfPointsLessFlat) < 0.005)
        {
            //同步作用，确保同时收到同一个点云的特征点以及IMU信息才进入
            newCornerPointsSharp = false;
            newCornerPointsLessSharp = false;
            newSurfPointsFlat = false;
            newSurfPointsLessFlat = false;
            newLaserCloudFullRes = false;
            newImuTrans = false;

            //将第一帧点云数据发送给laserMapping,从下一个点云数据开始处理
            if (!systemInited) 
            {
                //将cornerPointsLessSharp与laserCloudCornerLast交换,目的保存cornerPointsLessSharp的值下轮使用
                //需要交换,而不能简单地进行将cornerPointsLessSharp赋值给laserCloudCornerLast操作
                pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;
                cornerPointsLessSharp = laserCloudCornerLast;
                laserCloudCornerLast = laserCloudTemp;
                //将surfPointLessFlat与laserCloudSurfLast交换，目的保存surfPointsLessFlat的值下轮使用
                laserCloudTemp = surfPointsLessFlat;
                surfPointsLessFlat = laserCloudSurfLast;
                laserCloudSurfLast = laserCloudTemp;
                //构建kd-tree
                kdtreeCornerLast->setInputCloud(laserCloudCornerLast);//角点
                kdtreeSurfLast->setInputCloud(laserCloudSurfLast);//面点
                //将角点和面点分别发送给laserMapping
                sensor_msgs::PointCloud2 laserCloudCornerLast2;
                pcl::toROSMsg(*laserCloudCornerLast,laserCloudCornerLast2);
                laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
                laserCloudCornerLast2.header.frame_id = "rslidar";
                pubLaserCloudCornerLast.publish(laserCloudCornerLast2);

                sensor_msgs::PointCloud2 laserCloudSurfLast2;
                pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);
                laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
                laserCloudSurfLast2.header.frame_id = "rslidar";
                pubLaserCloudSurfLast.publish(laserCloudSurfLast2);
                
                //记住原点的横滚角和俯仰角,?????航向角呢
                transformSum[0] += imuPitchStart;
                transformSum[2] += imuRollStart;

                systemInited = true;
                continue;
            }
            
            //T平移量的初值赋值为加减速的位移量，为其梯度下降的方向（沿用上次转换的T（一个sweep匀速模型），同时在其基础上减去匀速运动位移，即只考虑加减速的位移量）
            //??????没看懂
            transform[3] -= imuVeloFromStartX * scanPeriod;
            transform[4] -= imuVeloFromStartY * scanPeriod;
            transform[5] -= imuVeloFromStartZ * scanPeriod;

            if (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100) 
            {
                //上一帧有足够多的角点和面点做点云配准
                std::vector<int> indices;
                //在收到消息时调用的回调函数不是去过一次Nan点了么????是否多余
                pcl::removeNaNFromPointCloud(*cornerPointsSharp,*cornerPointsSharp, indices);
                //当前帧中角点和面点的数量
                int cornerPointsSharpNum = cornerPointsSharp->points.size();
                int surfPointsFlatNum = surfPointsFlat->points.size();
                //LM优化,最多迭代25次
                for (int iterCount = 0; iterCount < 25; iterCount++)
                {
                    laserCloudOri->clear();
                    coeffSel->clear();
                    //处理当前点云中的曲率最大的特征点,从上个点云中曲率比较大的特征点中找两个最近距离点
                    //一个点使用kd-tree查找，另一个根据找到的点在其相邻线找另外一个最近距离的点
                    for(int i = 0; i < cornerPointsSharpNum; i++)
                    {
                        TransformToStart(&cornerPointsSharp->points[i], &pointSel);
                        //每迭代五次，重新查找最近点
                        if (iterCount % 5 == 0)
                        {
                            std::vector<int> indices;
                            pcl::removeNaNFromPointCloud(*laserCloudCornerLast,*laserCloudCornerLast, indices);
                            //kd-tree查找一个最近距离点，边沿点未经过体素栅格滤波，一般边沿点本来就比较少，不做滤波
                            //nearestKSearch是PCL中的K近邻域搜索，搜索上一时刻LessCornor的K邻域点
                            //搜索结果: pointSearchInd是索引; pointSearchSqDis是近邻对应的平方距离(以25作为阈值)
                            kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
                            int closestPointInd = -1, minPointInd2 = -1;

                            //寻找相邻线距离目标点距离最小的点
                            if (pointSearchSqDis[0] < 25)//找到的最近点距离的确很近的话
                            {
                                closestPointInd = pointSearchInd[0];
                                //提取最近点线号
                                int closestPointScan = int(laserCloudCornerLast->points[closestPointInd].intensity);

                                float pointSqDis, minPointSqDis2 = 25;//初始门槛值5米，可大致过滤掉scanID相邻，但实际线不相邻的值
                                //寻找距离目标点最近距离的平方和最小的点
                                for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++)
                                {
                                    //向scanID增大的方向查找
                                    if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan + 2.5)
                                    {
                                        //非相邻线
                                        break;
                                    }

                                    pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * 
                                                                (laserCloudCornerLast->points[j].x - pointSel.x) + 
                                                                (laserCloudCornerLast->points[j].y - pointSel.y) * 
                                                                (laserCloudCornerLast->points[j].y - pointSel.y) + 
                                                                (laserCloudCornerLast->points[j].z - pointSel.z) * 
                                                                (laserCloudCornerLast->points[j].z - pointSel.z);
                                    
                                    
                                    if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan) 
                                    {
                                        //确保两个点不在同一条scan上
                                        if (pointSqDis < minPointSqDis2)
                                        {
                                            //距离更近，要小于初始值5米,更新最小距离与点序
                                            minPointSqDis2 = pointSqDis;
                                            minPointInd2 = j;
                                        }
                                    }
                                }

                                //同理
                                for (int j = closestPointInd - 1; j >= 0; j--) 
                                {
                                    //向scanID减小的方向查找
                                    if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan - 2.5) 
                                    {
                                        break;
                                    }

                                    pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * 
                                                                (laserCloudCornerLast->points[j].x - pointSel.x) + 
                                                                (laserCloudCornerLast->points[j].y - pointSel.y) * 
                                                                (laserCloudCornerLast->points[j].y - pointSel.y) + 
                                                                (laserCloudCornerLast->points[j].z - pointSel.z) * 
                                                                (laserCloudCornerLast->points[j].z - pointSel.z);

                                    if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan)
                                    {
                                        if (pointSqDis < minPointSqDis2) 
                                        {
                                            minPointSqDis2 = pointSqDis;
                                            minPointInd2 = j;
                                        }
                                    }
                                }
                            }//寻找相邻线距离目标点距离最小的点

                            //记住组成线的点序
                            pointSearchCornerInd1[i] = closestPointInd;//kd-tree最近距离点，-1表示未找到满足的点
                            pointSearchCornerInd2[i] = minPointInd2;//另一个最近的，-1表示未找到满足的点

                        }//每迭代五次，重新查找最近点

                        if (pointSearchCornerInd2[i] >= 0) 
                        {
                            //说明两个点都找到了
                            tripod1 = laserCloudCornerLast->points[pointSearchCornerInd1[i]];
                            tripod2 = laserCloudCornerLast->points[pointSearchCornerInd2[i]];

                            //选择的特征点记为O，kd-tree最近距离点记为A，另一个最近距离点记为B
                            float x0 = pointSel.x;
                            float y0 = pointSel.y;
                            float z0 = pointSel.z;
                            float x1 = tripod1.x;
                            float y1 = tripod1.y;
                            float z1 = tripod1.z;
                            float x2 = tripod2.x;
                            float y2 = tripod2.y;
                            float z2 = tripod2.z;

                            //向量OA = (x0 - x1, y0 - y1, z0 - z1), 向量OB = (x0 - x2, y0 - y2, z0 - z2)，向量AB = （x1 - x2, y1 - y2, z1 - z2）
                            //向量OA OB的向量积(即叉乘)为：
                            //|  i      j      k  |
                            //|x0-x1  y0-y1  z0-z1|
                            //|x0-x2  y0-y2  z0-z2|
                            //模为：
                            float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
                                                    * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
                                                    + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
                                                    * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 
                                                    + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))
                                                    * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));
                            
                            //两个最近距离点之间的距离，即向量AB的模
                            float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));

                            //AB方向的单位向量与OAB平面的单位法向量的向量积在各轴上的分量（d的方向）
                            //x轴分量i
                            float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
                                            + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;
                            //y轴分量j
                            float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
                                            - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
                            //z轴分量k
                            float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 
                                            + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
                            //点到线的距离，d = |向量OA 叉乘 向量OB|/|AB|
                            float ld2 = a012 / l12;

                            //权重计算，距离越大权重越小，距离越小权重越大，得到的权重范围<=1
                            float s = 1;
                            if (iterCount >= 5) 
                            {
                                //5次迭代之后开始考虑权重因素
                                s = 1 - 1.8 * fabs(ld2);
                            }

                            //考虑权重
                            coeff.x = s * la;
                            coeff.y = s * lb;
                            coeff.z = s * lc;
                            coeff.intensity = s * ld2;

                            if (s > 0.1 && ld2 != 0) 
                            {
                                //只保留权重大的，也即距离比较小的点，同时也舍弃距离为零的
                                laserCloudOri->push_back(cornerPointsSharp->points[i]);
                                coeffSel->push_back(coeff);
                            }

                        }
                    }

                    //对本次接收到的曲率最小的点,从上次接收到的点云曲率比较小的点中找三点组成平面，
                    //一个使用kd-tree查找，另外一个在同一线上查找满足要求的，第三个在不同线上查找满足要求的
                    for (int i = 0; i < surfPointsFlatNum; i++) 
                    {
                        TransformToStart(&surfPointsFlat->points[i], &pointSel);
                        if (iterCount % 5 == 0)
                        {
                            //kd-tree最近点查找，在经过体素栅格滤波之后的平面点中查找，一般平面点太多，滤波后最近点查找数据量小
                            kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
                            int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
                            if (pointSearchSqDis[0] < 25)
                            {
                                closestPointInd = pointSearchInd[0];
                                int closestPointScan = int(laserCloudSurfLast->points[closestPointInd].intensity);
                                float pointSqDis, minPointSqDis2 = 25, minPointSqDis3 = 25;
                                for (int j = closestPointInd + 1; j < surfPointsFlatNum; j++)
                                {
                                    if(int(laserCloudSurfLast->points[j].intensity) > closestPointScan + 2.5)
                                    {
                                        break;
                                    }

                                    pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) * 
                                                                (laserCloudSurfLast->points[j].x - pointSel.x) + 
                                                                (laserCloudSurfLast->points[j].y - pointSel.y) * 
                                                                (laserCloudSurfLast->points[j].y - pointSel.y) + 
                                                                (laserCloudSurfLast->points[j].z - pointSel.z) * 
                                                                (laserCloudSurfLast->points[j].z - pointSel.z);
                                    
                                    if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScan)
                                    {
                                        //如果点的线号小于等于最近点的线号(应该最多取等，也即同一线上的点)
                                        if (pointSqDis < minPointSqDis2) 
                                        {
                                            minPointSqDis2 = pointSqDis;
                                            minPointInd2 = j;
                                        }
                                    }
                                    else
                                    {
                                        //如果点处在大于该线上
                                        if (pointSqDis < minPointSqDis3)
                                        {
                                            minPointSqDis3 = pointSqDis;
                                            minPointInd3 = j;
                                        }
                                    }
                                }

                                //同理
                                for (int j = closestPointInd - 1; j >= 0; j--) 
                                {
                                    if (int(laserCloudSurfLast->points[j].intensity) < closestPointScan - 2.5)
                                    {
                                        break;
                                    }

                                    pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) * 
                                                                (laserCloudSurfLast->points[j].x - pointSel.x) + 
                                                                (laserCloudSurfLast->points[j].y - pointSel.y) * 
                                                                (laserCloudSurfLast->points[j].y - pointSel.y) + 
                                                                (laserCloudSurfLast->points[j].z - pointSel.z) * 
                                                                (laserCloudSurfLast->points[j].z - pointSel.z);

                                    if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScan) 
                                    {
                                        if (pointSqDis < minPointSqDis2)
                                        {
                                            minPointSqDis2 = pointSqDis;
                                            minPointInd2 = j;
                                        }
                                    } 
                                    else 
                                    {
                                        if (pointSqDis < minPointSqDis3) 
                                        {
                                            minPointSqDis3 = pointSqDis;
                                            minPointInd3 = j;
                                        }
                                    }
                                }

                            }

                            pointSearchSurfInd1[i] = closestPointInd;//kd-tree最近距离点,-1表示未找到满足要求的点
                            pointSearchSurfInd2[i] = minPointInd2;//同一线号上的距离最近的点，-1表示未找到满足要求的点
                            pointSearchSurfInd3[i] = minPointInd3;//不同线号上的距离最近的点，-1表示未找到满足要求的点
                        }//每迭代5次

                        if (pointSearchSurfInd2[i] >= 0 && pointSearchSurfInd3[i] >= 0) 
                        {
                            //找到了三个点
                            tripod1 = laserCloudSurfLast->points[pointSearchSurfInd1[i]];//A点
                            tripod2 = laserCloudSurfLast->points[pointSearchSurfInd2[i]];//B点
                            tripod3 = laserCloudSurfLast->points[pointSearchSurfInd3[i]];//C点

                            //向量AB = (tripod2.x - tripod1.x, tripod2.y - tripod1.y, tripod2.z - tripod1.z)
                            //向量AC = (tripod3.x - tripod1.x, tripod3.y - tripod1.y, tripod3.z - tripod1.z)
                            //向量AB AC的向量积（即叉乘），得到的是法向量
                            //x轴方向分向量i
                            float pa = (tripod2.y - tripod1.y) * (tripod3.z - tripod1.z) 
                                            - (tripod3.y - tripod1.y) * (tripod2.z - tripod1.z);
                            //y轴方向分向量j
                            float pb = (tripod2.z - tripod1.z) * (tripod3.x - tripod1.x) 
                                            - (tripod3.z - tripod1.z) * (tripod2.x - tripod1.x);
                            //z轴方向分向量k
                            float pc = (tripod2.x - tripod1.x) * (tripod3.y - tripod1.y) 
                                            - (tripod3.x - tripod1.x) * (tripod2.y - tripod1.y);
                            
                            float pd = -(pa * tripod1.x + pb * tripod1.y + pc * tripod1.z);

                            //法向量的模
                            float ps = sqrt(pa * pa + pb * pb + pc * pc);

                            //pa pb pc为法向量各方向上的单位向量
                            pa /= ps;
                            pb /= ps;
                            pc /= ps;
                            pd /= ps;

                            //点到面的距离：向量OA与与法向量的点积除以法向量的模
                            float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;

                            //同理计算权重
                            float s = 1;
                            if (iterCount >= 5) 
                            {
                                s = 1 - 1.8 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x
                                        + pointSel.y * pointSel.y + pointSel.z * pointSel.z));
                            }

                            //考虑权重
                            coeff.x = s * pa;
                            coeff.y = s * pb;
                            coeff.z = s * pc;
                            coeff.intensity = s * pd2;

                            if (s > 0.1 && pd2 != 0)
                            {
                                //保存原始点与相应的系数
                                laserCloudOri->push_back(surfPointsFlat->points[i]);
                                coeffSel->push_back(coeff);
                            }
                        }
                    }

                    int pointSelNum = laserCloudOri->points.size();
                    //满足要求的特征点至少10个，特征匹配数量太少则弃用此帧数据
                    if (pointSelNum < 10) 
                    {
                        continue;
                    }

                    cv::Mat matA(pointSelNum, 6, CV_32F, cv::Scalar::all(0));
                    cv::Mat matAt(6, pointSelNum, CV_32F, cv::Scalar::all(0));
                    cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));
                    cv::Mat matB(pointSelNum, 1, CV_32F, cv::Scalar::all(0));
                    cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));
                    cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));

                    //计算matA,matB矩阵
                    for (int i = 0; i < pointSelNum; i++) 
                    {
                        pointOri = laserCloudOri->points[i];
                        coeff = coeffSel->points[i];

                        float s = 1;

                        float srx = sin(s * transform[0]);
                        float crx = cos(s * transform[0]);
                        float sry = sin(s * transform[1]);
                        float cry = cos(s * transform[1]);
                        float srz = sin(s * transform[2]);
                        float crz = cos(s * transform[2]);
                        float tx = s * transform[3];
                        float ty = s * transform[4];
                        float tz = s * transform[5];

                        float arx = (-s*crx*sry*srz*pointOri.x + s*crx*crz*sry*pointOri.y + s*srx*sry*pointOri.z 
                                        + s*tx*crx*sry*srz - s*ty*crx*crz*sry - s*tz*srx*sry) * coeff.x
                                        + (s*srx*srz*pointOri.x - s*crz*srx*pointOri.y + s*crx*pointOri.z
                                        + s*ty*crz*srx - s*tz*crx - s*tx*srx*srz) * coeff.y
                                        + (s*crx*cry*srz*pointOri.x - s*crx*cry*crz*pointOri.y - s*cry*srx*pointOri.z
                                        + s*tz*cry*srx + s*ty*crx*cry*crz - s*tx*crx*cry*srz) * coeff.z;

                        float ary = ((-s*crz*sry - s*cry*srx*srz)*pointOri.x 
                                        + (s*cry*crz*srx - s*sry*srz)*pointOri.y - s*crx*cry*pointOri.z 
                                        + tx*(s*crz*sry + s*cry*srx*srz) + ty*(s*sry*srz - s*cry*crz*srx) 
                                        + s*tz*crx*cry) * coeff.x
                                        + ((s*cry*crz - s*srx*sry*srz)*pointOri.x 
                                        + (s*cry*srz + s*crz*srx*sry)*pointOri.y - s*crx*sry*pointOri.z
                                        + s*tz*crx*sry - ty*(s*cry*srz + s*crz*srx*sry) 
                                        - tx*(s*cry*crz - s*srx*sry*srz)) * coeff.z;

                        float arz = ((-s*cry*srz - s*crz*srx*sry)*pointOri.x + (s*cry*crz - s*srx*sry*srz)*pointOri.y
                                        + tx*(s*cry*srz + s*crz*srx*sry) - ty*(s*cry*crz - s*srx*sry*srz)) * coeff.x
                                        + (-s*crx*crz*pointOri.x - s*crx*srz*pointOri.y
                                        + s*ty*crx*srz + s*tx*crx*crz) * coeff.y
                                        + ((s*cry*crz*srx - s*sry*srz)*pointOri.x + (s*crz*sry + s*cry*srx*srz)*pointOri.y
                                        + tx*(s*sry*srz - s*cry*crz*srx) - ty*(s*crz*sry + s*cry*srx*srz)) * coeff.z;

                        float atx = -s*(cry*crz - srx*sry*srz) * coeff.x + s*crx*srz * coeff.y 
                                        - s*(crz*sry + cry*srx*srz) * coeff.z;
            
                        float aty = -s*(cry*srz + crz*srx*sry) * coeff.x - s*crx*crz * coeff.y 
                                        - s*(sry*srz - cry*crz*srx) * coeff.z;
            
                        float atz = s*crx*sry * coeff.x - s*srx * coeff.y - s*crx*cry * coeff.z;

                        float d2 = coeff.intensity;

                        matA.at<float>(i, 0) = arx;
                        matA.at<float>(i, 1) = ary;
                        matA.at<float>(i, 2) = arz;
                        matA.at<float>(i, 3) = atx;
                        matA.at<float>(i, 4) = aty;
                        matA.at<float>(i, 5) = atz;
                        matB.at<float>(i, 0) = -0.05 * d2;

                    }

                    cv::transpose(matA, matAt);
                    matAtA = matAt * matA;
                    matAtB = matAt * matB;
                    //求解matAtA * matX = matAtB
                    cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);

                    if (iterCount == 0) 
                    {
                        //特征值1*6矩阵
                        cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0));
                        //特征向量6*6矩阵
                        cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0));
                        cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0));

                        //求解特征值/特征向量
                        cv::eigen(matAtA, matE, matV);
                        matV.copyTo(matV2);

                        isDegenerate = false;
                        //特征值取值门槛
                        float eignThre[6] = {10, 10, 10, 10, 10, 10};
                        for (int i = 5; i >= 0; i--) 
                        {
                            //从小到大查找
                            if (matE.at<float>(0, i) < eignThre[i])
                            {
                                //特征值太小，则认为处在兼并环境中，发生了退化
                                for (int j = 0; j < 6; j++) 
                                {
                                    //对应的特征向量置为0
                                    matV2.at<float>(i, j) = 0;
                                }
                                isDegenerate = true;
                            } 
                            else
                            {
                                break;
                            }
                        }

                        //计算P矩阵
                        matP = matV.inv() * matV2;
                    }

                    if (isDegenerate) 
                    {
                        //如果发生退化，只使用预测矩阵P计算
                        cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));
                        matX.copyTo(matX2);
                        matX = matP * matX2;
                    }

                    //累加每次迭代的旋转平移量
                    transform[0] += matX.at<float>(0, 0);
                    transform[1] += matX.at<float>(1, 0);
                    transform[2] += matX.at<float>(2, 0);
                    transform[3] += matX.at<float>(3, 0);
                    transform[4] += matX.at<float>(4, 0);
                    transform[5] += matX.at<float>(5, 0);

                    for(int i=0; i<6; i++)
                    {
                        if(isnan(transform[i]))//判断是否非数字
                        transform[i]=0;
                    }

                    //计算旋转平移量，如果很小就停止迭代
                    float deltaR = sqrt(
                                        pow(rad2deg(matX.at<float>(0, 0)), 2) +
                                        pow(rad2deg(matX.at<float>(1, 0)), 2) +
                                        pow(rad2deg(matX.at<float>(2, 0)), 2));
                    float deltaT = sqrt(
                                        pow(matX.at<float>(3, 0) * 100, 2) +
                                        pow(matX.at<float>(4, 0) * 100, 2) +
                                        pow(matX.at<float>(5, 0) * 100, 2));
                        
                    if(deltaR < 0.1 && deltaT < 0.1)
                    {
                        //迭代终止条件
                        break;
                    }

                }
            }

            float rx, ry, rz, tx, ty, tz;
            //求相对于原点的旋转量,垂直方向上1.05倍修正?
            AccumulateRotation(transformSum[0], transformSum[1], transformSum[2], 
                         -transform[0], -transform[1] * 1.05, -transform[2], rx, ry, rz);
            
            float x1 = cos(rz) * (transform[3] - imuShiftFromStartX) 
                    - sin(rz) * (transform[4] - imuShiftFromStartY);
            float y1 = sin(rz) * (transform[3] - imuShiftFromStartX) 
                    + cos(rz) * (transform[4] - imuShiftFromStartY);
            float z1 = transform[5] * 1.05 - imuShiftFromStartZ;

            float x2 = x1;
            float y2 = cos(rx) * y1 - sin(rx) * z1;
            float z2 = sin(rx) * y1 + cos(rx) * z1;

            //求相对于原点的平移量
            tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);
            ty = transformSum[4] - y2;
            tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);

            //根据IMU修正旋转量
            PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart, 
                        imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz);
            
            //得到世界坐标系下的转移矩阵
            transformSum[0] = rx;
            transformSum[1] = ry;
            transformSum[2] = rz;
            transformSum[3] = tx;
            transformSum[4] = ty;
            transformSum[5] = tz;

            //欧拉角转换成四元数
            geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(rz, -rx, -ry);

            //发布旋转四元数和平移量
            laserOdometry.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
            laserOdometry.pose.pose.orientation.x = -geoQuat.y;
            laserOdometry.pose.pose.orientation.y = -geoQuat.z;
            laserOdometry.pose.pose.orientation.z = geoQuat.x;
            laserOdometry.pose.pose.orientation.w = geoQuat.w;
            laserOdometry.pose.pose.position.x = tx;
            laserOdometry.pose.pose.position.y = ty;
            laserOdometry.pose.pose.position.z = tz;
            pubLaserOdometry.publish(laserOdometry);

            //广播新的平移旋转之后的坐标系(rviz)
            laserOdometryTrans.stamp_ = ros::Time().fromSec(timeSurfPointsLessFlat);
            laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
            laserOdometryTrans.setOrigin(tf::Vector3(tx, ty, tz));
            tfBroadcaster.sendTransform(laserOdometryTrans);

            //对点云的曲率比较大和比较小的点投影到扫描结束位置
            int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size();
            for (int i = 0; i < cornerPointsLessSharpNum; i++) 
            {
                TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]);
            }

            frameCount++;

            //点云全部点，每间隔一个点云数据相对点云最后一个点进行畸变校正
            if (frameCount >= skipFrameNum + 1)
            {
                int laserCloudFullResNum = laserCloudFullRes->points.size();
                for (int i = 0; i < laserCloudFullResNum; i++)
                {
                     TransformToEnd(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);
                }
            }

            //畸变校正之后的点作为last点保存,等下个点云进来进行匹配
            pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;
            cornerPointsLessSharp = laserCloudCornerLast;
            laserCloudCornerLast = laserCloudTemp;

            laserCloudTemp = surfPointsLessFlat;
            surfPointsLessFlat = laserCloudSurfLast;
            laserCloudSurfLast = laserCloudTemp;

            laserCloudCornerLastNum = laserCloudCornerLast->points.size();
            laserCloudSurfLastNum = laserCloudSurfLast->points.size();

            //点足够多就构建kd-tree，否则弃用此帧，沿用上一帧数据的kd-tree
            if (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100)
            {
                kdtreeCornerLast->setInputCloud(laserCloudCornerLast);
                kdtreeSurfLast->setInputCloud(laserCloudSurfLast);
            }

            //按照跳帧数publish边沿点，平面点以及全部点给laserMapping(每隔一帧发一次)
            if(frameCount >= skipFrameNum + 1)
            {
                frameCount = 0;
                sensor_msgs::PointCloud2 laserCloudCornerLast2;
                pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);
                laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
                laserCloudCornerLast2.header.frame_id = "rslidar";
                pubLaserCloudCornerLast.publish(laserCloudCornerLast2);

                sensor_msgs::PointCloud2 laserCloudSurfLast2;
                pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);
                laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
                laserCloudSurfLast2.header.frame_id = "rslidar";
                pubLaserCloudSurfLast.publish(laserCloudSurfLast2);

                sensor_msgs::PointCloud2 laserCloudFullRes3;
                pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);
                laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
                laserCloudFullRes3.header.frame_id = "rslidar";
                pubLaserCloudFullRes.publish(laserCloudFullRes3);

            }

        }
        status = ros::ok();
        rate.sleep();
    }

    // ROS_INFO("Laser_odometry...");
    return 0;
}